/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <libxml/parser.h>
#include <libxml/tree.h>
#include "A635Sensors.h"
#include "drawutils.h"
#include "World.h"
#include "owndebug.h"


#include "AC3DtoGL.hpp"
#include "GLProvider.h"



#define CHARGE_STATION_WATTS 500.00


static GLuint iListID; ///< GL display list ID
static CAC3DtoGL iCAC;

static CGLProvider* iGLProvider;




//*****************************************************************************

CObject* CA635Sensors::LoadMLMA(CWorld *world, xmlNode *node)
{
  CObject *object=NULL;
  // Parameters for a Line.
  xmlChar *x = xmlGetProp(node, (xmlChar *) "x");
  xmlChar *y = xmlGetProp(node, (xmlChar *) "y");
	xmlChar *z = xmlGetProp(node, (xmlChar *) "z");
  xmlChar *heading = xmlGetProp(node, (xmlChar *) "heading");
  xmlChar *name = xmlGetProp(node, (xmlChar *) "name");
  xmlChar *group = xmlGetProp(node, (xmlChar *) "MaCIGroup");
  
	xmlNode* laser_node=node->children;
	//laser_node->next;
	if(laser_node == NULL){
		dPrint(1,"Failed to read laser node!!!!!");
		exit(1);
	}
	
	int cnt = 0;
	TLaserPose3D lms291;
	TLaserPose3D lms111rear;
	TLaserPose3D lms111front;
	while(cnt<3){
		while(laser_node->type != XML_ELEMENT_NODE){
			laser_node=laser_node->next;
			if(laser_node == NULL){
				dPrint(1,"Puihin meni!");
				exit(1);
			}
		}
		
		if (!(xmlStrcmp(laser_node->name,(xmlChar*)"LMS111")) ){
			float lx,ly,lz,roll,pitch,yaw;
			lx = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "x") );
			ly = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "y") );
			lz = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "z") );
			
			roll = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "roll") );
			pitch = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "pitch") );
			yaw = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "yaw") );
			
			xmlChar *laserName = xmlGetProp(laser_node, (xmlChar *) "name");
			
			fprintf(stderr,"LMS111::%s (%.2f,%.2f,%.2f), (%.2f,%.2f,%.2f)\n",(const char*) laserName,lx,ly,lz,roll,pitch,yaw);
			
			if(!(xmlStrcmp(laserName,(xmlChar*)"Front"))){
				lms111front.set(lx,ly,lz,roll,pitch,yaw,(const char*)laserName);
			}else{
				lms111rear.set(lx,ly,lz,roll,pitch,yaw,(const char*)laserName);
			}
			
		}else if(!(xmlStrcmp(laser_node->name,(xmlChar*)"LMS291")) ){
			float lx,ly,lz,roll,pitch,yaw;
			lx = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "x") );
			ly = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "y") );
			lz = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "z") );
			
			roll = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "roll") );
			pitch = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "pitch") );
			yaw = atof((const char*)xmlGetProp(laser_node, (xmlChar *) "yaw") );
			
			xmlChar *laserName = xmlGetProp(laser_node, (xmlChar *) "name");
			fprintf(stderr,"LMS291::%s (%.2f,%.2f,%.2f), (%.2f,%.2f,%.2f)\n",(const char*) laserName,lx,ly,lz,roll,pitch,yaw);
			lms291.set(lx,ly,lz,roll,pitch,yaw,(const char*)laserName);
			
		}
		laser_node=laser_node->next;
		cnt++;
	}
	
	
  // Process.
  if (x && y && heading && (name || group)) {
    object=new CA635Sensors(world,
                    atof((const char*)x),
                    atof((const char*)y),
                    atof((const char*)heading),
                    name!=NULL ? (const char*)name : "",
                    group!=NULL ? (const char*)group : "",
														lms291, lms111rear, lms111front);
  } else {
    dPrint(1,"MLMA Error:Not enough parameters defined for A635Sensors-object!");
  }

  // Process MapFile
#ifdef MOTTI_MAP_DECODER_SUPPORT
#warning MottiMap Decoder support enabled.
  xmlChar *mottimapfilename = xmlGetProp(node, (xmlChar *) "MottiMapFilename");
  if (object && mottimapfilename) {
    CAGV *agv = static_cast<CAGV *>(object);
    agv->SetMottiMapFilename(reinterpret_cast<const char *>(mottimapfilename));
  }
  if (mottimapfilename) xmlFree(mottimapfilename);
#endif

  // Cleanup (Always do this to clean out properties which were found)
  if (x) xmlFree(x);
  if (y) xmlFree(y);
  if (heading) xmlFree(heading);
  if (name) xmlFree(name);
  if (group) xmlFree(group);

   
  if (object) {
    world->InsertObject(object);
  }
  
  return object;
}
//*****************************************************************************


CA635Sensors::CA635Sensors(CWorld* World, float x, float y, float heading, 
													 std::string gimnetName, std::string aMaCIGroup, 
													 TLaserPose3D navi, TLaserPose3D rear, TLaserPose3D front)
  : CObject(World,NULL),
    IdlePowerConsumption(50.0),
    DrivePowerConsumptionMultiplier(50.0),
    //MaxBatteryLevel(10000), // Very short while.
    MaxBatteryLevel(300 * 4*60*60), // 300W for 4hours (4*60*60s)
    gimi(),
    mcs(&gimi),
    SpeedCtrlServer(&mcs,0),
    PositionServer(&mcs,0),
    EnergyServer(&mcs,0),
    gimnetName(gimnetName),MaCIGroup(World->GetBaseMaCIGroup()+"."+aMaCIGroup),
    speed(0), angularSpeed(0), posX(x), posY(-y),
    heading((heading-90.0)*M_PI/180.0),
    batteryLevel(MaxBatteryLevel), powerConsumption(10),
    positionTimer(0), energyTimer(0),
    iWorld(World)
#ifdef MOTTI_MAP_DECODER_SUPPORT
  ,iMottiMap(),
    iMottiMapFilename(),
    iLastBatteryStatusUpdate(ownTime_get_ms())
#endif
{
  chassisMaterial.look.color[0]=1.0;
  chassisMaterial.look.color[1]=0.3;
  chassisMaterial.look.color[2]=0.1;

  forkMaterial.look.color[0]=0.1;
  forkMaterial.look.color[1]=0.1;
  forkMaterial.look.color[2]=0.1;
  
  lms111front = front;
	lms111rear = rear;
	lms291 = navi;
  
  
  Parts.push_back(chassis = new CBox(CHASSIS_WIDTH, CHASSIS_HEIGHT, CHASSIS_LENGTH,
                                     this, this->SpaceID, 0, &chassisMaterial, false));
  
  Parts.push_back(rightFork = new CBox(FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH,
                                       this, this->SpaceID, chassis->BodyID, &forkMaterial, false));
  
  Parts.push_back(leftFork = new CBox(FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH,
                                      this, this->SpaceID, chassis->BodyID, &forkMaterial, false));
  
  dGeomSetOffsetPosition(rightFork->GeomID, 
                         -FORK_GAP/2.0,
                         FORK_HEIGHT/2.0 - CHASSIS_HEIGHT/2.0,
                         FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
   
  dGeomSetOffsetPosition(leftFork->GeomID,
                         FORK_GAP/2.0,
                         FORK_HEIGHT/2.0 - CHASSIS_HEIGHT/2.0,
                         FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
   
  dBodySetPosition(chassis->BodyID,x,CHASSIS_HEIGHT/2.0+GROUND_CLEARANCE,-y);
  
  dMatrix3 R;
  dRFromAxisAndAngle(R,0,1,0,this->heading);
  dBodySetRotation(chassis->BodyID,R);
  /////////////////////////////////////////////////////////////////////////////////////////////
  /// LMS291 - Navi laser
  //#define SCANNER_OFFSET_Y 1.0
  //#define SCANNER_OFFSET_Z 1.95
  //#define SCANNER_OFFSET_X 0
  /// Y = Z, Z=-X, X = Y 
  //dReal pos[3]={lms291.x,lms291.y,lms291.z};
	dReal pos[3]={lms291.y,lms291.z, -lms291.x};
  dMatrix3 rot;
	///X_rot (pitch), -yaw_rot (z),roll_rot (y) 
	dRFromEulerAngles(rot,lms291.pitch,-(lms291.yaw+M_PI),lms291.roll);
  Children.push_back(LaserScanner=new CSick(World,this,chassis->BodyID,pos,rot,
                                            LASER_INTERVAL,&mcs,0,lms291.name));
	
	LaserScanner->SetDevicePosition(lms291.x, lms291.y,lms291.z, lms291.yaw);
  LaserScanner->setRayClusterVisible(true);
	dPrint(1,"Created LMS291::%s",lms291.name.c_str());
	/////////////////////////////////////////////////////////////////////////////////////////////
	/// REAR SCANNER
	/////////////////////////////////////////////////////////////////////////////////////////////
	//dReal pos2[3]={REAR_SCANNER_OFFSET_X,REAR_SCANNER_OFFSET_Y,REAR_SCANNER_OFFSET_Z};
	
	//dReal pos2[3]={lms111rear.x,lms111rear.z,-lms111rear.y};
	dReal pos2[3]={lms111rear.y,lms111rear.z,-lms111rear.x};
  //dRFrom2Axes(rot, -1,0,0, 0,1,0);
	///X_rot (pitch), -yaw_rot (z),roll_rot (y) 
	dRFromEulerAngles(rot,lms111rear.pitch,-(lms111rear.yaw+M_PI),lms111rear.roll);
  Children.push_back(rearLaserScanner=new CSick(World,this,chassis->BodyID,pos2,rot,
                                            LASER_INTERVAL,&mcs,1,lms111rear.name));
	rearLaserScanner->SetDevicePosition(lms111rear.x, lms111rear.y,lms111rear.z, lms111rear.yaw);
	rearLaserScanner->setRayClusterVisible(true);
	dPrint(1,"Created LMS111::%s",lms111rear.name.c_str());
	/////////////////////////////////////////////////////////////////////////////////////////////
	/// FRONT SCANNER
	/////////////////////////////////////////////////////////////////////////////////////////////
	//dReal pos3[3]={FRONT_SCANNER_OFFSET_X,FRONT_SCANNER_OFFSET_Y,FRONT_SCANNER_OFFSET_Z};
	dReal pos3[3]={lms111front.y,lms111front.z,-lms111front.x};
	dRFromEulerAngles(rot,lms111front.pitch,-(lms111front.yaw+M_PI),lms111front.roll);
  Children.push_back(frontLaserScanner=new CSick(World,this,chassis->BodyID,pos3,rot,
                                            LASER_INTERVAL,&mcs,2,lms111front.name));
	frontLaserScanner->SetDevicePosition(lms111front.x, lms111front.y,lms111front.z, lms111front.yaw);
	frontLaserScanner->setRayClusterVisible(true);
	dPrint(1,"Created LMS111::%s",lms111front.name.c_str());
}
//*****************************************************************************
#ifdef MOTTI_MAP_DECODER_SUPPORT

void CA635Sensors::SetMottiMapFilename(const std::string &aMottiMapFilename)
{
  iMottiMapFilename = aMottiMapFilename;
}
#endif
//*****************************************************************************

void CA635Sensors::Activate()
{
  if (MaCIGroup.length()) {
    mcs.SetGroupName(MaCIGroup);

  } else {
    mcs.SetGroupName(gimnetName);

  }
  
  if (gimnetName.empty()) {
    gimnetName=MaCI::GetStaticClientName(MaCIGroup+"FSRSimAGV");
  }
  
  SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  PositionServer.SetInterfaceInstanceName("Motion");
  EnergyServer.SetInterfaceInstanceName("Energy");
  
  connectGIMI();
  
  MaCI::EMaCIError e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
    {
      dPrint(1,"Couldnt open SpeedCtrlServer");
    } else {
    SpeedCtrlServer.SetSpeedLimits(MAX_MOTOR_SPEED, MAX_ANGULAR_SPEED, MAX_ACCELERATION,
                                   -MAX_MOTOR_SPEED, -MAX_ANGULAR_SPEED, -MAX_ACCELERATION);
  }
  e=PositionServer.Open();
  if (e!=MaCI::KMaCIOK) {
    dPrint(1,"Couldnt open PositionServer");
  }
  
  e=EnergyServer.Open();
  if (e!=MaCI::KMaCIOK) {
    dPrint(1,"Couldnt open EnergyServer");
  }
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++) {
    (*i)->Activate();
  }

#ifdef MOTTI_MAP_DECODER_SUPPORT
  if (iMottiMapFilename.length() && iMottiMap.OpenMottiMapFile(iMottiMapFilename) == false) {
    dPrintLCRed(ODERROR,"Failed to open MottiMap '%s', even mottimap was enabled! MottiMap data not in use.", 
                iMottiMapFilename.c_str());
    iMottiMapFilename.clear();
  }
#endif

}
//*****************************************************************************

void CA635Sensors::Update(float StepSize)
{
  
  const dReal *P, *R;
  dReal v[3]={0,0,1};
  dReal r[3];
  float h;
  P=dBodyGetPosition(chassis->BodyID);
  R=dBodyGetRotation(chassis->BodyID);
  RotateVector(R,v,r);
  //#warning "NOTE: Using inverted y-axis and heading"
  h=atan2(-r[2],-r[0]);
  
  posX=P[0];
  posY=P[2];
  heading=h;

  MaCI::SpeedCtrl::CSpeedCtrlData d;
  if (batteryLevel > 0.00 && SpeedCtrlServer.WaitCommand(d,0,true)==MaCI::KMaCIOK) {
    const MaCI::SpeedCtrl::TCommand *c=d.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c) {
      switch (c->cmd) {
      case MaCI::SpeedCtrl::KCommandSetSpeed:
        s=d.GetSpeed();
        if (s) {
          dPrint(1,"Got set speed %f,%f",s->speed,s->angularSpeed);
          speed=s->speed;
          if (speed==0)
            speed=0.000001; //for some reason ode cant handle exact zero
          angularSpeed=s->angularSpeed;
          //s->acceleration;
          //          dPrint(1,"Set speed");
          //dBodySetLinearVel(chassis->BodyID,0,0,speed);
          dBodySetLinearVel(chassis->BodyID,speed*cos(heading),0,speed*sin(heading));
          dBodySetAngularVel(chassis->BodyID,0,angularSpeed,0);
        }
        break;
      case MaCI::SpeedCtrl::KCommandSetStop:
        speed=0;
        angularSpeed=0;
        //s->acceleration;
        dPrint(1,"Set speed");
        //dBodySetLinearVel(chassis->BodyID,0,0,speed);
        dBodySetLinearVel(chassis->BodyID,0,0,0);
        dBodySetAngularVel(chassis->BodyID,0,0,0);
        break;
      case MaCI::SpeedCtrl::KCommandTimeout:
        speed=0;
        angularSpeed=0;
        dBodySetLinearVel(chassis->BodyID,0,0,0);
        dBodySetAngularVel(chassis->BodyID,0,0,0);
        break;
      }
    }
  } else if (batteryLevel == 0.00) {
    #warning WHEN BATTERY RUNS OUT - THE AGV STARTS 'SLIDING'..
    //    dPrint(ODTEST,"Won't move anymore - Battery empty!");
    speed=0.000001;
    angularSpeed=0.000001;
  }

  MaCI::SpeedCtrl::CSpeedCtrlData speedEvent;
  gim::binbag::CBinBag bag;
  speedEvent.EncodeTo(&bag); 
  speedEvent.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
  speedEvent.SetSpeed(MaCI::SpeedCtrl::TSpeed(speed,angularSpeed));
  speedEvent.SetTimestamp(MaCI::Common::TTimestamp());
  SpeedCtrlServer.SendSpeedEvent(speedEvent);
  
  if (positionTimer>=POSITION_INTERVAL)
    {
      MaCI::Position::CPositionData pd;
      bag.Clear();
      pd.EncodeTo(&bag);
      pd.SetTimestamp(MaCI::Common::TTimestamp());
      MaCI::Position::TPose2D pose(posX,-posY,-heading);
      pose.NormalizeAngle();

      pd.SetPose2D(pose);
      pd.SetVariance2D(MaCI::Position::TVariance2D(0,0,0));
      pd.SetProbability(MaCI::Position::TProbability(1));

      PositionServer.SendPositionEvent(pd);

      positionTimer=0;
    }
  
  positionTimer+=StepSize;
  
  
  if (energyTimer>=ENERGY_INTERVAL) {
    MaCI::Energy::CEnergyData ed;
    ed.CreateInternalBinBag();
    ed.SetTimestamp(MaCI::Common::TTimestamp());
    // Add a dummy event
    ed.SetEnergyEvent(MaCI::Energy::TEnergyEvent(MaCI::Energy::KEventTypeEnergyData, 
                                                 (batteryLevel/MaxBatteryLevel) * 100.0, 
                                                 MaxBatteryLevel, 
                                                 powerConsumption));
    
    // Broadcast!
    EnergyServer.SendEnergyEvent(ed);
    
    energyTimer=0;
  }
  energyTimer+=StepSize;
  
  updateBattery(StepSize);
  /*
    heading=heading+StepSize*angularSpeed;
    posX=posX+cos(heading)*speed*StepSize;
    posY=posY-sin(heading)*speed*StepSize;
  */
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
    {
      (*i)->Update(StepSize);
    }
  
}
//*****************************************************************************

void CA635Sensors::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
  
}
//*****************************************************************************
//*****************************************************************************


void CA635Sensors::renderStrokeFontString(
                                  float x, 
                                  float y, 
                                  float z, 
                                  void *font, 
                                  const char *string) {  

  
  if(!iGLProvider)
    {
      assert(iWorld != NULL);
      iGLProvider = iWorld->GetGLProvider();

    }
  

  if(iGLProvider != NULL)
    {
  
      float camx, camy, camz, campan, camtilt;

      //printf("Getting camera parameters!\n");
      iGLProvider->GetCameraParams(camx, camy, camz, campan, camtilt);
      


      // We define the rotation around y (up) by compairing x and y positions


      //   -camx = unitY
      //   -camz = unitX

      float xydist = sqrt( (camx + posX) * (camx + posX) +
                           (camz + posY) * (camz + posY));

      float xyzdist = sqrt( xydist * xydist + camy * camy);
      
      float xrot = sin(-camy / xyzdist);
      xrot = xrot * 360 / (2 * M_PI);


      float yrot;
      
      float headingadd = heading;

      while(headingadd < -M_PI)
        headingadd += 2*M_PI;

      while(headingadd > M_PI)
        headingadd -= 2*M_PI;

      if(headingadd >= 0 && headingadd <= M_PI)
        {
          headingadd -= M_PI/2;

        } else
        {
          headingadd = (headingadd - M_PI/2);
        }


      yrot = atan2( posX + camx,  posY + camz); 
      yrot = (yrot + headingadd) * 360 / (2 * M_PI);


      

      /**
      printf("Params: %lf %lf %lf %lf %lf\n", camx, camy, camz, campan, camtilt);
      printf("Unit Position: x: %lf y: %lf\n", posY, posX);
      printf("yrot: %lf \n", yrot);
      */

      const char *c;
      glPushMatrix();
      glTranslatef(x, y,z);

      glColor3f(1.0, 0.0, 0.0);
      glRotatef(yrot, 0, 1.0, 0);
      glRotatef(xrot, 1.0, 0, 0);
      
      // float scaler = (1/xyzdist)*0.1; 
      float scaler = 0.01;

      glScalef(scaler, scaler , scaler);
      glLineWidth(2.0f);


      for (c=string; *c != '\0'; c++) {
    glutStrokeCharacter(font, *c);
      }
      glPopMatrix();
      
    }
}

//*****************************************************************************
//*****************************************************************************



static bool gListCreated = false;

void CA635Sensors::Draw()
{


  if(!gListCreated)
    {
      gListCreated = true;

      // We read a .ac model file for the MottiPbV
      // Loads a .ac file
      //CAC3DtoGL cac;
      
      //  GLuint modelDL = iCAC.createDisplayList(name);
      if(!iCAC.CreateModelFromFile("models/agv.ac"))
        {
          std::cout << "Model failed to load!, exiting" << std::endl;
          exit(1);
        }
      iCAC.CreateDisplayList();
      iListID = iCAC.GetDisplayListID();

      printf("CREATED GL LIST WITH ID %d\n", iListID);
      
    }




  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++) {
    (*i)->Draw();
  }
  
  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++){
    (*i)->Draw();
  }
  
  
  dReal M[16];
  
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  
  //glLoadIdentity();
  
  chassis->GetGeomTransMatrix(M);
  glMultMatrixf(M);
  

  // glRotatef(-90,0.0f,1.0f,0.0f);
  
  glScalef(0.15, 0.15, 0.15);
  glTranslatef(0.0, -1.8, 0.0);
  //glCallList(iListID);
  // glRotatef(90,0.0f,1.0f,0.0f);
  
  //glScalef(0.5, 0.5, 0.5);
  glTranslatef(0.0, 3.0, 0.0);

  renderStrokeFontString(0, 10, 0, GLUT_STROKE_ROMAN, MaCIGroup.c_str());
  


  //   glTranslatef(posX, 0, posY);
  //   glRotatef(heading*180.0/M_PI+90,0,1,0);
  
  /*  glBegin(GL_LINES);
  //x
  glColor3f(1,0,0);
  glVertex3f(0,0,0);
  glVertex3f(1,0,0);
  //z
  glColor3f(0,0,1);
  glVertex3f(0,0,0);
  glVertex3f(0,1,0);
  //y
  glColor3f(0,1,0);
  glVertex3f(0,0,0);
  glVertex3f(0,0,-1);
  glEnd();*/
  
  /*
    glColor3f(1.0,0.3,0.1);
    drawBox( CHASSIS_WIDTH, CHASSIS_HEIGHT, CHASSIS_LENGTH, 0, CHASSIS_HEIGHT/2.0 + GROUND_CLEARANCE, 0);
  
    glColor3f(0.1,0.1,0.1);
    drawBox( FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH, -FORK_GAP/2.0, 
    FORK_HEIGHT/2.0 + GROUND_CLEARANCE, FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
  
    drawBox( FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH, FORK_GAP/2.0, 
    FORK_HEIGHT/2.0 + GROUND_CLEARANCE, FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
  */
  glPopMatrix();
  
}
//*****************************************************************************

void CA635Sensors::updateBattery(const float &aStepSize)
{
  // Power consumption now.
  powerConsumption = (IdlePowerConsumption + (DrivePowerConsumptionMultiplier * fabs(speed)));

#ifdef MOTTI_MAP_DECODER_SUPPORT
  if (!iMottiMapFilename.empty()) {
    // Enabled :)
    if (iMottiMap.IsInsideChargingStation(posX, -posY, 1.0)) {
      powerConsumption = -CHARGE_STATION_WATTS; // Charge at 50Watts :)
    }
  }

#else
  powerConsumption = 0.00;
#endif

  // Calculate current battery level by power consumption
  batteryLevel -= powerConsumption * aStepSize;
  if (batteryLevel > 0 && batteryLevel <= MaxBatteryLevel) {
  } else if (batteryLevel <= 0) {
    batteryLevel = 0.00;
    
  } else {
    batteryLevel = MaxBatteryLevel;
    
  }
  if (ownTime_get_ms_since(iLastBatteryStatusUpdate) > 5000) {
    if (powerConsumption > 0) {
      dPrint(ODTEST,"%s: Battery discharging at %.2f Watts, Empty in %.4f hours (%.0f Joules left)", 
             MaCIGroup.c_str(), 
             powerConsumption, 
             batteryLevel / powerConsumption / 60.0 / 60.0,
             batteryLevel);
      
    } else {
      dPrint(ODTEST,"%s: Battery charging at %.2f Watts, Full in %.4f hours (%.0f Joules left to charge)", 
             MaCIGroup.c_str(), 
             -powerConsumption, 
             (MaxBatteryLevel-batteryLevel) / -powerConsumption / 60.0 / 60.0,
             (MaxBatteryLevel-batteryLevel));

    }

    iLastBatteryStatusUpdate = ownTime_get_ms();
  }




}
//*****************************************************************************

void CA635Sensors::connectGIMI()
{
  if (!SimWorld->ConnectGimi(&gimi,gimnetName)) {
    dPrint(ODTEST, "Failed to Connect to GIMI! Fatal error!");
    abort();
    
  }
}
//*****************************************************************************
//*****************************************************************************

